/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filters.h"
#include "math.h"
#include "kalman_2d.h"
#include "fc_state.h"
#include "spl06_api.h"

#define BARO_AVERAGE_SIZE 9
#define GROUN_SAMPLE_COUNT 50
// laggedMovingAverage_t baro_average;
// pt1Filter_t baroLpf;
// Kalman_t baroKal;
// kalman_2d_t baro_kalman;
// bool baro_init = false;
bool ground_init = false;
float baro_buffer[BARO_AVERAGE_SIZE];
float baro_ave[BARO_AVERAGE_SIZE];
// float alt_sum = 0;
uint8_t ground_count = 0;
float groud_pressure = 0;

extern float F1_wave[12];

/**
 * @brief
 *
 * @param P
 * @return float (cm)
 */
float calculate_altitude(float P, float groundpressure, float T)
{
#if 0
    // const float P0 = 101325;  // 海平面气压，单位：帕斯卡
    const float L = 0.0065f;      // 气温垂直递减率，单位：开尔文/厘米
    const float T0 = 273.15f + T; // 海平面气温，单位：开尔文
    const float g = 9.8f;         // 重力加速度，单位：米/秒²
    const float M = 0.0289644f;   // 空气平均摩尔质量，单位：千克/摩尔
    const float R = 8.314f;       // 普适气体常量，单位：焦耳/（摩尔·开尔文）

    // 应用变形后的公式计算海拔
    // return (T0 * 15384.6153846154f) * (1.f - pow((P / groundpressure), 0.190385015919613f));
    return (T0 * 100.f / L) * (1 - pow((P / groundpressure), (R * L) / (g * M)));
#else
    return 4433000.f * (1.f - pow(P / groundpressure, 1.f / 5.255f));
#endif
}
void sensor_baro_updata(void)
{
    baro_t *baro = &fc_sensor_data.baro;
    baro->temperature = spl06_dev_get_temperature();
    baro->pressure = spl06_dev_get_pressure();
    if (FC_STATE_CHECK(FC_IMU_CALIBRATING))
    {
        ground_count = 0;
    }
    if (ground_count < GROUN_SAMPLE_COUNT)
    {
        if (ground_count == 0)
        {
            groud_pressure = baro->pressure;
        }
        ground_count++;

        groud_pressure += 0.5f * (baro->pressure - groud_pressure);
    }
    float altitude = calculate_altitude(baro->pressure, groud_pressure, baro->temperature);
    baro->altitude = altitude;
}
